#include <ros/ros.h>
#include "mcu_correspond/mcu_data_subscriber.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <cmath>
#include <locale>
#include <algorithm>
#include "my_pcl/my_pcl.h"

class SinglePoleDetector {
private:
    ros::NodeHandle nh_;
    McuDataSubscriber mcu_data_;

    // 点云订阅器
    ros::Subscriber pointcloud_sub_;
    
    // 滤波参数
    static const int FILTER_WINDOW_SIZE = 7;
    std::vector<float> pole_x_history_;
    std::vector<float> pole_y_history_;
    bool pole_locked_;
    float locked_pole_x_;
    float locked_pole_y_;
    int consecutive_detections_;
    
    // 杆子参数配置
    const double STICK_MIN_DIAMETER = 0.05;  // 5cm - 最小直径
    const double STICK_MAX_DIAMETER = 0.18;  // 18cm - 最大直径
    const double min_length = 0.07;  // 最小高度 7cm

    // 检测参数
    struct DetectionParams {
        int lock_threshold = 5; // 连续检测到多少次后锁定
        double position_tolerance = 10; // 位置变化容忍度
    } params_;

    // 简单的杆子信息结构
    struct SimplePoleInfo {
        double x_position;
        double y_position;
        double diameter;
        double height;
        double distance;
    };

public:
    SinglePoleDetector(ros::NodeHandle& nh) : nh_(nh), mcu_data_(nh), pole_locked_(false), consecutive_detections_(0) {
        // 订阅点云数据
        pointcloud_sub_ = nh_.subscribe("/livox/map", 1, &SinglePoleDetector::pointCloudCallback, this);
        
        ROS_INFO("Single Pole Detection System initialized. Min diameter: %.1fcm, Max diameter: %.1fcm", 
                 STICK_MIN_DIAMETER*100, STICK_MAX_DIAMETER*100);
    }
    
    // 找到单根杆子的函数 - 选择最佳的一根
    SimplePoleInfo findSinglePole(const std::vector<pcl::PointCloud<PointType>::Ptr>& merged_clusters,
                                 double min_diameter, double max_diameter, double min_height)
    {
        SimplePoleInfo best_pole;
        best_pole.distance = std::numeric_limits<double>::max(); // 初始化为最大值
        bool found_valid_pole = false;
        
        ROS_INFO("========== 单杆检测分析开始 ==========");
        ROS_INFO("检测参数: 直径范围 %.1f-%.1fcm, 最小高度 %.1fcm", 
                 min_diameter*100, max_diameter*100, min_height*100);
        ROS_INFO("总聚类数: %zu", merged_clusters.size());

        int cluster_index = 0;
        for (const auto& cluster : merged_clusters)
        {
            // 快速边界框计算
            PointType min_pt, max_pt;
            pcl::getMinMax3D(*cluster, min_pt, max_pt);

            double diameter = std::sqrt(std::pow(max_pt.x - min_pt.x, 2) + 
                                       std::pow(max_pt.y - min_pt.y, 2));
            double height = max_pt.z - min_pt.z;
            
            ROS_INFO("聚类 %d: 直径=%.1fcm, 高度=%.1fcm, 点数=%zu", 
                     cluster_index, diameter*100, height*100, cluster->points.size());
            
            if (diameter >= min_diameter && diameter <= max_diameter && height > min_height)
            {
                Eigen::Vector4f centroid;
                pcl::compute3DCentroid(*cluster, centroid);
                double distance = std::sqrt(centroid[0]*centroid[0] + centroid[1]*centroid[1]);

                // 选择距离最近的杆子
                if (distance < best_pole.distance) {
                    best_pole.x_position = centroid[0];
                    best_pole.y_position = centroid[1];
                    best_pole.diameter = diameter;
                    best_pole.height = height;
                    best_pole.distance = distance;
                    found_valid_pole = true;
                    
                    ROS_INFO("  ✓ 找到更优杆子! 质心: (%.3f, %.3f), 距离: %.3f", 
                             centroid[0], centroid[1], distance);
                }
            }
            else
            {
                ROS_INFO("  ✗ 不符合条件");
            }
            cluster_index++;
        }
        
        ROS_INFO("========== 单杆检测分析结束 ==========");
        if (found_valid_pole) {
            ROS_INFO("最终选择杆子: 位置(%.3f, %.3f), 距离=%.3f, 直径=%.1fcm", 
                     best_pole.x_position, best_pole.y_position, best_pole.distance, best_pole.diameter*100);
        } else {
            ROS_INFO("未找到符合条件的杆子");
            best_pole.distance = -1; // 标记为无效
        }
        
        return best_pole;
    }
    
    // 更新杆子位置并进行滤波
    void updatePolePosition(float new_x, float new_y)
    {
        // 如果已经锁定且新位置变化太大，重置检测
        if (pole_locked_) {
            float distance = sqrt(pow(new_x - locked_pole_x_, 2) + pow(new_y - locked_pole_y_, 2));
            if (distance > params_.position_tolerance) {
                ROS_WARN("杆子位置变化过大(%.3f > %.3f)，重置检测", distance, params_.position_tolerance);
                resetDetection();
                return;
            }
            return; // 位置锁定后不再更新
        }
        
        // 添加到历史记录
        pole_x_history_.push_back(new_x);
        pole_y_history_.push_back(new_y);
        
        if (pole_x_history_.size() > FILTER_WINDOW_SIZE) {
            pole_x_history_.erase(pole_x_history_.begin());
            pole_y_history_.erase(pole_y_history_.begin());
        }
        
        consecutive_detections_++;
        
        // 检查是否满足锁定条件
        if (consecutive_detections_ >= params_.lock_threshold && 
            pole_x_history_.size() >= params_.lock_threshold) {
            
            // 计算滤波后的位置
            locked_pole_x_ = 0.0;
            locked_pole_y_ = 0.0;
            for (size_t i = 0; i < pole_x_history_.size(); ++i) {
                locked_pole_x_ += pole_x_history_[i];
                locked_pole_y_ += pole_y_history_[i];
            }
            locked_pole_x_ /= pole_x_history_.size();
            locked_pole_y_ /= pole_y_history_.size();
            
            pole_locked_ = true;
            ROS_INFO("杆子位置已锁定: (%.3f, %.3f)", locked_pole_x_, locked_pole_y_);
        } else {
            ROS_INFO("检测进度: %d/%d", consecutive_detections_, params_.lock_threshold);
        }
    }
    
    void resetDetection()
    {
        consecutive_detections_ = 0;
        pole_x_history_.clear();
        pole_y_history_.clear();
        pole_locked_ = false;
        ROS_INFO("重置杆子检测状态");
    }
    
    // 检测并更新杆子位置
    void detectAndUpdatePole(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
        try {
            // 点云处理 - 使用ObjectFind库
            pcl::PointCloud<PointType>::Ptr cloud(new pcl::PointCloud<PointType>);
            pcl::fromROSMsg(*cloud_msg, *cloud);
            
            // 使用深度优化的ObjectFind库处理点云并发布彩色聚类
            auto clusters = processCloudsForStickDetection(cloud, "/clusters_cloud", cloud_msg->header.frame_id);
            
            // 找到最佳的单根杆子
            SimplePoleInfo pole = findSinglePole(clusters, STICK_MIN_DIAMETER, STICK_MAX_DIAMETER, min_length);
            
            if (pole.distance > 0) { // 找到有效杆子
                updatePolePosition(pole.x_position, pole.y_position);
            } else {
                if (!pole_locked_) {
                    ROS_WARN("未检测到符合条件的杆子，继续搜索...");
                }
            }
            
        } catch (const std::exception& e) {
            ROS_ERROR("杆子检测错误: %s", e.what());
        }
    }
    
    // 发送杆子位置数据到MCU
    void publishPoleData() {
        if (pole_locked_) {
            // 发布检测结果
            mcu_data_.publishDoubleData("isPoleFound", 1.0);
            mcu_data_.publishDoubleData("pole_x", locked_pole_x_);
            mcu_data_.publishDoubleData("pole_y", locked_pole_y_);
            
            ROS_INFO_THROTTLE(2.0, "发布杆子数据: isPoleFound=1, x=%.3f, y=%.3f", 
                              locked_pole_x_, locked_pole_y_);
        } else {
            ROS_INFO_THROTTLE(5.0, "发布杆子数据: isPoleFound=0 (未锁定)");
        }
    }
    
    // 点云回调函数
    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) {
        // 持续检测并更新杆子位置
        detectAndUpdatePole(cloud_msg);
        
        // 发送杆子数据到MCU
        publishPoleData();
    }
};

int main(int argc, char** argv) {
    std::setlocale(LC_ALL, "zh_CN.UTF-8");

    ros::init(argc, argv, "single_pole_detector");
    ros::NodeHandle nh;
    
    // Create single pole detector
    SinglePoleDetector detector(nh);
    
    ROS_INFO("Single Pole Detection System started - Using MCU data publishing");
    ROS_INFO("Publishing: isPoleFound, pole_x, pole_y to MCU serial interface");
    ros::spin();
    return 0;
}
